#include "mpu.h"

#include <math.h>

#include "err.h"
#include "hal_mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

#undef Handler
#define Handler Mpu_HandleTypeDef*

#undef handler
#define handler (**(Mpu_HandleTypeDef**)&handle)

// q30格式,四元数long转float时的除数.
#define q30 1073741824.0f

uint8_t Mpu_Init(Mpu_HandleTypeDef* handle) {
  static uint8_t inited = 0;
  if (!inited && mpu_dmp_init()) return 1;
  inited = 1;
  return 0;
}

void Mpu_Update(Mpu_HandleTypeDef* handle) {
  unsigned long sensor_timestamp;
  short gyro[3], accel[3], sensors;
  long quat[4];
  float fq[4];
  unsigned char more;

  if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more))
    return;

  if (sensors & INV_WXYZ_QUAT) {
    for (int i = 0; i < 4; i++) fq[i] = quat[i] / q30;

    handle->yaw =
        atan2(2 * (fq[1] * fq[2] + fq[0] * fq[3]),
              fq[0] * fq[0] + fq[1] * fq[1] - fq[2] * fq[2] - fq[3] * fq[3]) *
        180.0f / M_PI;
    handle->pitch =
        asin(-2.0f * (fq[1] * fq[3] - fq[0] * fq[2])) * 180.0f / M_PI;
    handle->roll =
        atan2(2.0f * (fq[0] * fq[1] + fq[2] * fq[3]),
              fq[0] * fq[0] - fq[1] * fq[1] - fq[2] * fq[2] + fq[3] * fq[3]) *
        180.0f / M_PI;
  }
}
